Simultaneous Localization and Map Building with Modified System State

The full covariance solution to simultaneous localization and map building based on extended Kalman filter requires update time quadratic in the number of landmarks in the map. In order to improve the computational efficiency, this paper reorganizes system state vector and system models. The state of mobile robot is redefined and represented indirectly. The higher dimensional system models and covariance matrix can be represented with two lower dimensional submatrices. An optimization solution is proposed based on this property. The computational requirement and memory requirement are decreased by half. The covariance matrix is fully updated without any approximation during estimation. The optimization solution is consistent and convergent theoretically and realistically. The experiment also compares the performance of optimization solution with the full covariance solution. All these techniques have been implemented on mobile robot ATRVII equipped with 2D laser rangefinder SICK.